
#include "funcdef.h"


/* ========================================================================== */
/*                           FUNCTION DEFINITIONS                             */
/* ========================================================================== */

/* \fn
 *
 * \brief   Function to MS5611 pressure sensor driver.
 *
 */


/* Initialized values */
Uint8 MS5611_CRC = 0;
Uint8 MS5611_CRC_REF = 0;
static Uint16 C[8] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};



static int MS5611_init(TSK_Ctrl_b * tsk);


/*--------------MS5611_sample_tsk()-----------------------------
 * This task is set up to run periodically by pending on MS5611_sem,
 * which is update by PRD module.
 *------------------------------------------------ ------*/
void MS5611_sample_tsk(void)
{
	int error = 0;
	double data[4];
	TSK_Ctrl_b *self = &(SCB.TCB[TSK_MS5611]);


	SEM_pend(&MS5611_sem, SYS_FOREVER);
	// Check if the sensor has been initialized
	while(self->status != INITIALISED){
		error = MS5611_init(self);
		if(error){
			error = GIO_control(self->inhd,I2c_IOCTL_DEVICE_SOFT_RESET,NULL);
			if(error){
				LOG_printf(self->trace, "RESET IIC failed: %d\n", error);
			}else{
				LOG_printf(self->trace, "RESET IIC succeed\n");
			}
		}
	}


	vTskTime(self,RESET_TIME);
	/* Initialize task's STS object to current time */
	TSK_settime(TSK_self());
	/* Sensor initialized, perform sampling operations */
	for(;;){
		SEM_pend(&MS5611_sem, SYS_FOREVER);
		//vTskTime(self,SET_PREV_TIME);
		STS_set(&sts_MS5611, SCB.TIME_STAMP);
		error = MS5611_GETDATA(self->inhd,self->outhd, data, C, FIRST_ORDER);
		if(!error){
			if(SCB.TIME_STAMP < 100.00){
				//samples sensor offsets in the first 100 ms
				vNavData.P_offset = data[0];
				vNavData.T_offset = data[1];
				vNavData.H_offset = data[2];
			}
			vNavData.P = data[0];
			vNavData.T = data[1];
			vNavData.BAROH = data[2];
			vNavData.BAROH_R = vNavData.BAROH - vNavData.H_offset;
		}else{
			error = GIO_control(self->inhd,I2c_IOCTL_DEVICE_SOFT_RESET,NULL);
			if(error){
				LOG_printf(self->trace, "RESET IIC failed: %d\n", error);
			}else{
				LOG_printf(self->trace, "RESET IIC succeed\n");
			}
			LOG_printf(self->trace, "MS5611_GETDATA fail: %d", error);
		}
		vTskTime(self,DELTA_TIME);
		STS_delta(&sts_MS5611,SCB.TIME_STAMP);
		/* Get time difference and
		add it to task's STS object */
		TSK_deltatime(TSK_self());
		TSK_yield();
	}
}

/*--------------MS5611_init()-----------------------------
 * This function performs deviceID check
 * and read out initial values
 * from
 *------------------------------------------------ ------*/
static int MS5611_init(TSK_Ctrl_b * tsk){
	int status = 0;
	int i = 0;
	for(i = 0; i < 8;i++){
		C[i]= MS5611_Prom_RD(tsk->inhd, tsk->outhd, &status, i);
		if(status){
			LOG_printf(tsk->trace, "MS5611_Prom_RD Error: %d\n",status);
			return status;
		}
	}
	MS5611_CRC_REF = 0x000F & C[7];
	C[7] = C[7] & 0xFF00;
	MS5611_CRC = MS5611_CRC4(C);
	if( MS5611_CRC != MS5611_CRC_REF )
	{
		LOG_printf(tsk->trace, "MS5611 CRC Error!\n");
	}else{
		tsk->status = INITIALISED;
		LOG_printf(tsk->trace, "MS5611 Initialized Successfully\n");
	}
	return status;
}

